From 833b9918be91eb845d06df2e62dba40a0e0bee26 Mon Sep 17 00:00:00 2001 From: parkrrrr Date: Fri, 14 Nov 2003 15:37:13 +0000 Subject: [PATCH] Moved great circle functionality to its own module --- gpsbabel/Makefile | 8 +- gpsbabel/arcdist.c | 171 +-------------------------------------- gpsbabel/grtcirc.c | 192 ++++++++++++++++++++++++++++++++++++++++++++ gpsbabel/grtcirc.h | 25 ++++++ gpsbabel/position.c | 17 ++-- 5 files changed, 230 insertions(+), 183 deletions(-) create mode 100644 gpsbabel/grtcirc.c create mode 100644 gpsbabel/grtcirc.h diff --git a/gpsbabel/Makefile b/gpsbabel/Makefile index 6907f8b10..d398a5796 100644 --- a/gpsbabel/Makefile +++ b/gpsbabel/Makefile @@ -32,7 +32,8 @@ JEEPS=jeeps/gpsapp.o jeeps/gpscom.o \ COLDSYNC=coldsync/util.o coldsync/pdb.o -LIBOBJS = queue.o route.o waypt.o filter_vecs.o util.o vecs.o mkshort.o csv_util.o \ +LIBOBJS = queue.o route.o waypt.o filter_vecs.o util.o vecs.o mkshort.o \ + csv_util.o grtcirc.o \ $(COLDSYNC) $(GARMIN) $(JEEPS) $(FMTS) $(FILTERS) OBJS = main.o $(LIBOBJS) @@ -85,7 +86,7 @@ release: # Machine generated from here down. -arcdist.o: arcdist.c defs.h queue.h +arcdist.o: arcdist.c defs.h queue.h grtcirc.h cetus.o: cetus.c defs.h queue.h coldsync/palm.h coldsync/pdb.h copilot.o: copilot.c defs.h queue.h coldsync/palm.h coldsync/pdb.h csv_util.o: csv_util.c defs.h queue.h csv_util.h @@ -104,6 +105,7 @@ gpilots.o: gpilots.c defs.h queue.h coldsync/palm.h coldsync/pdb.h gpspilot.o: gpspilot.c defs.h queue.h coldsync/palm.h coldsync/pdb.h gpsutil.o: gpsutil.c defs.h queue.h magellan.h gpx.o: gpx.c defs.h queue.h +grtcirc.o: grtcirc.c defs.h queue.h holux.o: holux.c defs.h queue.h holux.h internal_styles.o: internal_styles.c defs.h queue.h magnav.o: magnav.c defs.h queue.h coldsync/palm.h coldsync/pdb.h @@ -115,7 +117,7 @@ mkshort.o: mkshort.c defs.h queue.h navicache.o: navicache.c defs.h queue.h pcx.o: pcx.c defs.h queue.h garmin_tables.h polygon.o: polygon.c defs.h queue.h -position.o: position.c defs.h queue.h +position.o: position.c defs.h queue.h grtcirc.h psitrex.o: psitrex.c defs.h queue.h garmin_tables.h psp.o: psp.c defs.h queue.h queue.o: queue.c queue.h diff --git a/gpsbabel/arcdist.c b/gpsbabel/arcdist.c index e4bf0e7f4..e4f447347 100644 --- a/gpsbabel/arcdist.c +++ b/gpsbabel/arcdist.c @@ -19,12 +19,8 @@ */ #include -#include #include "defs.h" - -#ifndef M_PI -# define M_PI 3.14159265358979323846 -#endif +#include "grtcirc.h" #define MYNAME "Arc filter" @@ -49,171 +45,6 @@ arglist_t arcdist_args[] = { {0, 0, 0, 0} }; -static double gcdist( double lat1, double lon1, double lat2, double lon2 ) { - return acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2)); -} - - -static void crossproduct( double x1, double y1, double z1, - double x2, double y2, double z2, - double *xa, double *ya, double *za ) { - *xa = y1*z2-y2*z1; - *ya = z1*x2-z2*x1; - *za = x1*y2-y1*x2; -} - -static double dotproduct( double x1, double y1, double z1, - double x2, double y2, double z2 ) { - return (x1*x2+y1*y2+z1*z2); -} - -static double linedist(double lat1, double lon1, - double lat2, double lon2, - double lat3, double lon3 ) { - - static double _lat1 = -9999; - static double _lat2 = -9999; - static double _lon1 = -9999; - static double _lon2 = -9999; - - static double x1,y1,z1; - static double x2,y2,z2; - static double xa,ya,za,la; - - double x3,y3,z3; - double xp,yp,zp,lp; - - double xa1,ya1,za1; - double xa2,ya2,za2; - - double d1, d2; - double c1, c2; - - double dot; - - int newpoints; - - /* degrees to radians */ - lat1 *= M_PI/180.0; lon1 *= M_PI/180.0; - lat2 *= M_PI/180.0; lon2 *= M_PI/180.0; - lat3 *= M_PI/180.0; lon3 *= M_PI/180.0; - - newpoints = 1; - if ( lat1 == _lat1 && lat2 == _lat2 && lon1 == _lon1 && lon2 == _lon2) { - newpoints = 0; - } - else { - _lat1 = lat1; - _lat2 = lat2; - _lon1 = lon1; - _lon2 = lon2; - } - - /* polar to ECEF rectangular */ - if ( newpoints ) { - x1 = cos(lon1)*cos(lat1); y1 = sin(lat1); z1 = sin(lon1)*cos(lat1); - x2 = cos(lon2)*cos(lat2); y2 = sin(lat2); z2 = sin(lon2)*cos(lat2); - } - x3 = cos(lon3)*cos(lat3); y3 = sin(lat3); z3 = sin(lon3)*cos(lat3); - - if ( newpoints ) { - /* 'a' is the axis; the line that passes through the center of the earth - * and is perpendicular to the great circle through point 1 and point 2 - * It is computed by taking the cross product of the '1' and '2' vectors.*/ - crossproduct( x1, y1, z1, x2, y2, z2, &xa, &ya, &za ); - la = sqrt(xa*xa+ya*ya+za*za); - - if ( la ) { - xa /= la; - ya /= la; - za /= la; - } - } - if ( la ) { - - /* dot is the component of the length of '3' that is along the axis. - * What's left is a non-normalized vector that lies in the plane of - * 1 and 2. */ - - dot = dotproduct(x3,y3,z3,xa,ya,za); - - xp = x3-dot*xa; - yp = y3-dot*ya; - zp = z3-dot*za; - - lp = sqrt(xp*xp+yp*yp+zp*zp); - - if ( lp ) { - xp /= lp; - yp /= lp; - zp /= lp; - - crossproduct(x1,y1,z1,xp,yp,zp,&xa1,&ya1,&za1); - d1 = dotproduct( xa1, ya1, za1, xa, ya, za ); - - crossproduct(xp,yp,zp,x2,y2,z2,&xa2,&ya2,&za2); - d2 = dotproduct( xa2, ya2, za2, xa, ya, za ); - - if ( d1 >= 0 && d2 >= 0 ) { - /* rather than call gcdist and all its sines and cosines and - * worse, we can get the angle directly. It's the arctangent - * of the length of the component of vector 3 along the axis - * divided by the length of the component of vector 3 in the - * plane. We already have both of those numbers. - * - * atan2 would be overkill because lp and fabs(dot) are both - * known to be positive. */ - - return atan( fabs(dot)/lp ); - } - - /* otherwise, get the distance from the closest endpoint */ - c1 = dotproduct( x1,y1,z1,xp,yp,zp ); - c2 = dotproduct( x2,y2,z2,xp,yp,zp ); - d1 = labs(d1); - d2 = labs(d2); - - /* This is a hack. d$n$ is proportional to the sine of the angle - * between point $n$ and point p. That preserves orderedness up - * to an angle of 90 degrees. c$n$ is proportional to the cosine - * of the same angle; if the angle is over 90 degrees, c$n$ is - * negative. In that case, we flop the sine across the y=1 axis - * so that the resulting value increases as the angle increases. - * - * This only works because all of the points are on a unit sphere. */ - - if ( c1 < 0 ) { - d1 = 2 - d1; - } - if ( c2 < 0 ) { - d2 = 2 - d2; - } - - if ( labs(d1) < labs(d2)) { - return gcdist(lat1,lon1,lat3,lon3); - } - else { - return gcdist(lat2,lon2,lat3,lon3); - } - } - else { - /* lp is 0 when 3 is 90 degrees from the great circle */ - return M_PI/2; - } - } - else { - /* la is 0 when 1 and 2 are either the same point or 180 degrees apart */ - dot = dotproduct(x1,y1,z1,x2,y2,z2); - if ( dot >= 0 ) { - return gcdist(lat1,lon1,lat3,lon3); - } - else { - return 0; - } - } - return 0; -} - #define BADVAL 999999 void diff --git a/gpsbabel/grtcirc.c b/gpsbabel/grtcirc.c new file mode 100644 index 000000000..0106e1216 --- /dev/null +++ b/gpsbabel/grtcirc.c @@ -0,0 +1,192 @@ +/* + Great Circle utility functions + + Copyright (C) 2002 Robert Lipe, robertlipe@usa.net + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111 USA + + */ +#include +#include +#include "defs.h" + +#ifndef M_PI +# define M_PI 3.14159265358979323846 +#endif + +static void crossproduct( double x1, double y1, double z1, + double x2, double y2, double z2, + double *xa, double *ya, double *za ) { + *xa = y1*z2-y2*z1; + *ya = z1*x2-z2*x1; + *za = x1*y2-y1*x2; +} + +static double dotproduct( double x1, double y1, double z1, + double x2, double y2, double z2 ) { + return (x1*x2+y1*y2+z1*z2); +} + +double gcdist( double lat1, double lon1, double lat2, double lon2 ) { + return acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2)); +} + +double linedist(double lat1, double lon1, + double lat2, double lon2, + double lat3, double lon3 ) { + + static double _lat1 = -9999; + static double _lat2 = -9999; + static double _lon1 = -9999; + static double _lon2 = -9999; + + static double x1,y1,z1; + static double x2,y2,z2; + static double xa,ya,za,la; + + double x3,y3,z3; + double xp,yp,zp,lp; + + double xa1,ya1,za1; + double xa2,ya2,za2; + + double d1, d2; + double c1, c2; + + double dot; + + int newpoints; + + /* degrees to radians */ + lat1 *= M_PI/180.0; lon1 *= M_PI/180.0; + lat2 *= M_PI/180.0; lon2 *= M_PI/180.0; + lat3 *= M_PI/180.0; lon3 *= M_PI/180.0; + + newpoints = 1; + if ( lat1 == _lat1 && lat2 == _lat2 && lon1 == _lon1 && lon2 == _lon2) { + newpoints = 0; + } + else { + _lat1 = lat1; + _lat2 = lat2; + _lon1 = lon1; + _lon2 = lon2; + } + + /* polar to ECEF rectangular */ + if ( newpoints ) { + x1 = cos(lon1)*cos(lat1); y1 = sin(lat1); z1 = sin(lon1)*cos(lat1); + x2 = cos(lon2)*cos(lat2); y2 = sin(lat2); z2 = sin(lon2)*cos(lat2); + } + x3 = cos(lon3)*cos(lat3); y3 = sin(lat3); z3 = sin(lon3)*cos(lat3); + + if ( newpoints ) { + /* 'a' is the axis; the line that passes through the center of the earth + * and is perpendicular to the great circle through point 1 and point 2 + * It is computed by taking the cross product of the '1' and '2' vectors.*/ + crossproduct( x1, y1, z1, x2, y2, z2, &xa, &ya, &za ); + la = sqrt(xa*xa+ya*ya+za*za); + + if ( la ) { + xa /= la; + ya /= la; + za /= la; + } + } + if ( la ) { + + /* dot is the component of the length of '3' that is along the axis. + * What's left is a non-normalized vector that lies in the plane of + * 1 and 2. */ + + dot = dotproduct(x3,y3,z3,xa,ya,za); + + xp = x3-dot*xa; + yp = y3-dot*ya; + zp = z3-dot*za; + + lp = sqrt(xp*xp+yp*yp+zp*zp); + + if ( lp ) { + xp /= lp; + yp /= lp; + zp /= lp; + + crossproduct(x1,y1,z1,xp,yp,zp,&xa1,&ya1,&za1); + d1 = dotproduct( xa1, ya1, za1, xa, ya, za ); + + crossproduct(xp,yp,zp,x2,y2,z2,&xa2,&ya2,&za2); + d2 = dotproduct( xa2, ya2, za2, xa, ya, za ); + + if ( d1 >= 0 && d2 >= 0 ) { + /* rather than call gcdist and all its sines and cosines and + * worse, we can get the angle directly. It's the arctangent + * of the length of the component of vector 3 along the axis + * divided by the length of the component of vector 3 in the + * plane. We already have both of those numbers. + * + * atan2 would be overkill because lp and fabs(dot) are both + * known to be positive. */ + + return atan( fabs(dot)/lp ); + } + + /* otherwise, get the distance from the closest endpoint */ + c1 = dotproduct( x1,y1,z1,xp,yp,zp ); + c2 = dotproduct( x2,y2,z2,xp,yp,zp ); + d1 = labs(d1); + d2 = labs(d2); + + /* This is a hack. d$n$ is proportional to the sine of the angle + * between point $n$ and point p. That preserves orderedness up + * to an angle of 90 degrees. c$n$ is proportional to the cosine + * of the same angle; if the angle is over 90 degrees, c$n$ is + * negative. In that case, we flop the sine across the y=1 axis + * so that the resulting value increases as the angle increases. + * + * This only works because all of the points are on a unit sphere. */ + + if ( c1 < 0 ) { + d1 = 2 - d1; + } + if ( c2 < 0 ) { + d2 = 2 - d2; + } + + if ( labs(d1) < labs(d2)) { + return gcdist(lat1,lon1,lat3,lon3); + } + else { + return gcdist(lat2,lon2,lat3,lon3); + } + } + else { + /* lp is 0 when 3 is 90 degrees from the great circle */ + return M_PI/2; + } + } + else { + /* la is 0 when 1 and 2 are either the same point or 180 degrees apart */ + dot = dotproduct(x1,y1,z1,x2,y2,z2); + if ( dot >= 0 ) { + return gcdist(lat1,lon1,lat3,lon3); + } + else { + return 0; + } + } + return 0; +} + diff --git a/gpsbabel/grtcirc.h b/gpsbabel/grtcirc.h new file mode 100644 index 000000000..94c354a08 --- /dev/null +++ b/gpsbabel/grtcirc.h @@ -0,0 +1,25 @@ +/* + Great Circle utility functions + + Copyright (C) 2002 Robert Lipe, robertlipe@usa.net + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111 USA + + */ +double gcdist( double lat1, double lon1, double lat2, double lon2 ); + +double linedist(double lat1, double lon1, + double lat2, double lon2, + double lat3, double lon3 ); diff --git a/gpsbabel/position.c b/gpsbabel/position.c index c3ea570bd..a1f016a80 100644 --- a/gpsbabel/position.c +++ b/gpsbabel/position.c @@ -21,6 +21,7 @@ #include #include #include "defs.h" +#include "grtcirc.h" #ifndef M_PI # define M_PI 3.14159265358979323846 @@ -66,16 +67,12 @@ arglist_t radius_args[] = { static double gc_distance(double lat1, double lon1, double lat2, double lon2) { - double rlat1, rlat2, rlon1, rlon2; - - /* convert to radians */ - rlat1 = (lat1 * M_PI) / 180.0; - rlon1 = (lon1 * M_PI) / 180.0; - rlat2 = (lat2 * M_PI) / 180.0; - rlon2 = (lon2 * M_PI) / 180.0; - - return (acos((sin(rlat1) * sin(rlat2)) + - (cos(rlat1) * cos(rlat2) * cos(rlon1 - rlon2)))); + return gcdist( + (lat1 * M_PI) / 180.0, + (lon1 * M_PI) / 180.0, + (lat2 * M_PI) / 180.0, + (lon2 * M_PI) / 180.0 + ); } static int -- 2.30.2